function occ_map = update_occupancy_grid(occ_map, pose, scan, max_range, fov, num_beams, resolution, map_size)
% 更新占据栅格地图 (使用log-odds)
% 输入: occ_map-当前地图, pose-位姿, scan-激光扫描
% 输出: occ_map-更新后地图

    x = pose(1);
    y = pose(2);
    theta = pose(3);
    
    % log-odds参数
    l_occ = 0.7;    % 占据的log-odds增量
    l_free = -0.4;  % 空闲的log-odds增量
    l_max = 5.0;    % 最大log-odds值
    l_min = -5.0;   % 最小log-odds值
    
    % 激光束角度
    angle_min = theta - fov/2;
    angle_max = theta + fov/2;
    angles = linspace(angle_min, angle_max, num_beams);
    
    grid_size = size(occ_map, 1);
    
    for i = 1:num_beams
        angle = angles(i);
        range = scan(i);
        
        if range >= max_range
            continue;
        end
        
        % 终点（障碍物位置）
        end_x = x + range * cos(angle);
        end_y = y + range * sin(angle);
        
        % Bresenham光线投射
        [free_cells, occ_cell] = bresenham_line(x, y, end_x, end_y, resolution, map_size, grid_size);
        
        % 更新空闲栅格
        for j = 1:size(free_cells, 1)
            gx = free_cells(j, 1);
            gy = free_cells(j, 2);
            if gx >= 1 && gx <= grid_size && gy >= 1 && gy <= grid_size
                occ_map(gy, gx) = occ_map(gy, gx) + l_free;
                occ_map(gy, gx) = max(occ_map(gy, gx), l_min);
            end
        end
        
        % 更新占据栅格
        if ~isempty(occ_cell)
            gx = occ_cell(1);
            gy = occ_cell(2);
            if gx >= 1 && gx <= grid_size && gy >= 1 && gy <= grid_size
                occ_map(gy, gx) = occ_map(gy, gx) + l_occ;
                occ_map(gy, gx) = min(occ_map(gy, gx), l_max);
            end
        end
    end
end

function [free_cells, occ_cell] = bresenham_line(x0, y0, x1, y1, resolution, map_size, grid_size)
% Bresenham直线算法
    
    % 世界坐标转栅格坐标
    % 使用转置+axis xy显示时，需要正确映射坐标
    % 矩阵存储：occ_map(row, col) 对应世界坐标 (col*res, row*res)
    % 转置后显示：occ_map'使得x对应列，y对应行，且axis xy使y向上
    gx0 = round(x0 / resolution) + 1;
    gy0 = round(y0 / resolution) + 1;
    gx1 = round(x1 / resolution) + 1;
    gy1 = round(y1 / resolution) + 1;
    
    dx = abs(gx1 - gx0);
    dy = abs(gy1 - gy0);
    
    if gx0 < gx1
        sx = 1;
    else
        sx = -1;
    end
    
    if gy0 < gy1
        sy = 1;
    else
        sy = -1;
    end
    
    err = dx - dy;
    
    gx = gx0;
    gy = gy0;
    
    free_cells = [];
    
    while true
        if gx == gx1 && gy == gy1
            break;
        end
        
        % 记录空闲栅格
        free_cells = [free_cells; gx, gy];
        
        e2 = 2 * err;
        if e2 > -dy
            err = err - dy;
            gx = gx + sx;
        end
        if e2 < dx
            err = err + dx;
            gy = gy + sy;
        end
    end
    
    % 终点为占据栅格
    occ_cell = [gx1, gy1];
end

